#!/bin/bash

# 松灵小车传感器监控系统启动脚本

echo "==================== 松灵小车传感器监控系统 ===================="
echo "正在启动传感器监控系统..."

# 设置环境变量
export LANG=zh_CN.UTF-8
export LC_ALL=zh_CN.UTF-8

# 检查ROS环境
if [ -z "$ROS_DISTRO" ]; then
    echo "错误: ROS环境未设置，请先source ROS环境"
    echo "请运行: source /opt/ros/melodic/setup.bash"
    exit 1
fi

# 检查catkin工作空间
if [ -z "$CATKIN_WS" ]; then
    echo "警告: CATKIN_WS环境变量未设置，使用默认路径"
    CATKIN_WS="$HOME/catkin_ws"
fi

if [ ! -d "$CATKIN_WS" ]; then
    echo "错误: catkin工作空间不存在: $CATKIN_WS"
    exit 1
fi

# 进入工作空间并source环境
cd $CATKIN_WS
source devel/setup.bash

echo "ROS环境: $ROS_DISTRO"
echo "工作空间: $CATKIN_WS"
echo ""

# 检查roscore是否运行
if ! pgrep -x "roscore" > /dev/null; then
    echo "启动roscore..."
    roscore &
    sleep 3
fi

# 显示菜单
echo "请选择要启动的监控模式:"
echo "1) 完整监控系统 (所有传感器)"
echo "2) 仅速度显示"
echo "3) 仅相机显示"
echo "4) 仅激光雷达显示"
echo "5) 综合状态监控"
echo "6) 退出"
echo ""

read -p "请输入选择 (1-6): " choice

case $choice in
    1)
        echo "启动完整监控系统..."
        roslaunch scout_sensor_monitor scout_sensor_monitor.launch
        ;;
    2)
        echo "启动速度显示..."
        roslaunch scout_sensor_monitor velocity_only.launch
        ;;
    3)
        echo "启动相机显示..."
        roslaunch scout_sensor_monitor camera_only.launch
        ;;
    4)
        echo "启动激光雷达显示..."
        roslaunch scout_sensor_monitor lidar_only.launch
        ;;
    5)
        echo "启动综合状态监控..."
        rosrun scout_sensor_monitor sensor_monitor_node
        ;;
    6)
        echo "退出程序"
        exit 0
        ;;
    *)
        echo "无效选择，退出程序"
        exit 1
        ;;
esac

echo "监控系统已启动"
echo "按 Ctrl+C 停止程序"
